/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMM
* @author     Andre Meixner
* @copyright  2017 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
*/

#include <MMM/Motion/Motion.h>
#include "MMMSynchronizeConfiguration.h"
#include <MMM/Motion/MotionWriterXML.h>
#include <MMM/Motion/MotionReaderXML.h>

#include <string>
#include <vector>

using namespace MMM;

int main(int argc, char *argv[]) {
    MMM_INFO << " --- MMMSynchronize --- " << endl;
    MMMSynchronizeConfiguration *configuration = new MMMSynchronizeConfiguration();
    if (!configuration->processCommandLine(argc, argv))
    {
        MMM_ERROR << "Error while processing command line, aborting..." << endl;
        return -1;
    }

    try {
        MMM::MotionReaderXMLPtr motionReader(new MMM::MotionReaderXML(configuration->sensorPluginPaths));
        MMM_INFO << "Reading motion file '" << configuration->inputMotionPath << "'!" << std::endl;
        MMM::MotionList motions = motionReader->loadAllMotions(configuration->inputMotionPath);

        for (auto motion : motions) motion->synchronizeSensorMeasurements(configuration->timeFrequency);
	
        MMM_INFO << "Writing motions to " << configuration->outputMotionPath << std::endl;
        MMM::MotionWriterXML::writeMotion(motions, configuration->outputMotionPath);

        return 0;
    }
    catch (MMM::Exception::MMMException &e) {
        MMM_ERROR << e.what() << std::endl;
        return -1;
    }
}
